/*
 * File:   PrimaryStateSearchIR.c
 */

#include <p24FJ64GA002.h>
#include "states.h"
#include "pwm.h"
#include "sensors.h"

/* ************************************************************************** */
/* IR Aquisition States */
#define IR_SEARCH_STATE_BASELINE 1
#define IR_SEARCH_STATE_INIT     2
#define IR_SEARCH_STATE_ROTATE   3

/* IR Pivot States */
#define IR_PIVOT_CCW 1
#define IR_PIVOT_CW  2

/* IR Tolerance constant and macros */
#define TOLERANCE_WINDOW              0.25f
#define TOLERANCE_WINDOW_FLOOR(X)     ((1.0f - TOLERANCE_WINDOW) * X)
#define TOLERANCE_WINDOW_CEILING(X)   ((1.0f + TOLERANCE_WINDOW) * X)
#define CALCULATE_TOLERANCES(X, F, C) F = TOLERANCE_WINDOW_FLOOR(X); \
                                      C = TOLERANCE_WINDOW_CEILING(X);

/* Use pow(2,n) number of samples so we can bit shift to divide */
#define ADC_IR_SAMPLES         16
/* If we change the number of samples, remember to change this too. */
#define ADC_IR_SAMPLES_DIVIDER 4
#define OC_RISING_EDGE_TIME    2000
#define HIGH_PASS_FILTER_PARAM 62
#define ROTATIONS              3

/* ************************************************************************** */
static unsigned irRotateCount    = 0;
static unsigned irCompareFloor   = 0;
static unsigned irCompareCeiling = 0;
static unsigned irSamples        = 0;
static unsigned irSearchState    = IR_SEARCH_STATE_BASELINE;
static unsigned irPivotState     = IR_PIVOT_CCW;
static unsigned irValues[ADC_IR_SAMPLES];

// Declarations for internal states of the IR search functionality
void IRBaselineReading();
void IRSearchInit();
void IRSearchRotate();

/* ************************************************************************** */
void PrimaryStateSearchIR(void) {
    switch (irSearchState) {
        case IR_SEARCH_STATE_BASELINE:
            IRBaselineReading();
            break;
        case IR_SEARCH_STATE_INIT:
            IRSearchInit();
            break;
        case IR_SEARCH_STATE_ROTATE:
            IRSearchRotate();
            break;
        default:
            irSearchState = IR_SEARCH_STATE_BASELINE;
    }
}

// Get baseline IR reading
void IRBaselineReading() {
    if (_DONE) {
        irValues[0] = GetCurrentSensorValue(SENSOR_IR);
        CALCULATE_TOLERANCES(irValues[0], irCompareFloor, irCompareCeiling)
        irSearchState = IR_SEARCH_STATE_INIT;
    }
}

// Initializes all state variables to their respective defaults
void IRSearchInit() {
    OC1R = (unsigned) OC_RISING_EDGE_TIME;
    irRotateCount = 0;
    irSamples = 0;
    irPivotState = IR_PIVOT_CCW;
    irSearchState = IR_SEARCH_STATE_ROTATE;
    PivotCCW();
}

// Tracks the IR intensity until it finds the highest value
void IRSearchRotate() {
    if (irRotateCount == 5) {
        Stop();
        SetCurrentPrimaryState(PRIMARY_STATE_LEAVE_TAPE);
    } else if (irSamples < ADC_IR_SAMPLES) {
        unsigned irValue = GetCurrentSensorValue(SENSOR_IR);
        // High pass filter --- omit noise
        if (irValue > HIGH_PASS_FILTER_PARAM) {
            irValues[irSamples++] = irValue;
        }
    } else {
        unsigned i = 0, average = 0;
        for (; i < ADC_IR_SAMPLES; i++) {
            average += irValues[i];
        }
        average = average >> ADC_IR_SAMPLES_DIVIDER;
        if (average >= irCompareCeiling) {
            if (irPivotState == IR_PIVOT_CW) {
                Stop();
                irPivotState = IR_PIVOT_CCW;
                PivotCCW();
            }
            CALCULATE_TOLERANCES(average, irCompareFloor, irCompareCeiling)
        } else if (average <= irCompareFloor) {
            if (irPivotState == IR_PIVOT_CCW) {
                Stop();
                irRotateCount++;
                irPivotState = IR_PIVOT_CW;
                PivotCW();
            }
            CALCULATE_TOLERANCES(average, irCompareFloor, irCompareCeiling)
        }
        irSamples = 0;
    }
}
